#include "datatx_task.h"
#include "ROS.h"
#include "bsp_dwt.h"
#include "referee.h"
#include "dbus.h"
#include "can_controller.h"

#define CAN_REMOTE_RC_S						0x210
#define CAN_REMOTE_RC_C						0x211
#define CAN_REMOTE_RC_M           0x212

#define CAN_Refereedata2Gimbal    0x209


void Chassis_mode_changing(void);
void Get_routeArray(float start_point,float target_point,int8_t delta_array[49]);

void Datatx_task(void const * argument)
{
	
	
	static uint32_t cnt_last,count;
	static float   dt_odom;
	
	uint16_t outpos_enmyhp,outpos_selfhp=0;

	
	static int8_t delta_array_x[49],delta_array_y[49];
	static float map_x,map_y,map_x_last,map_y_last;
	
	static uint8_t  enemy_outpostarrive_flag;
	static bool_t reborn,remote_reborn;
	static uint16_t Bullet_amount;
	static uint8_t remote_Bullet_times,remote_Blood_times,bullet_times;
	
 
	//状态机
	static uint8_t step_blood;
	static uint8_t step_projectile;
	static uint8_t allowance_17mm_flag;
	
	static uint8_t step_Bullet;
	static uint8_t step_outpost;
	static uint8_t step_defense;
	static uint8_t step_dog;
	
	static uint16_t outpost_time;
	while(1)
	{
//		cnt_last =DWT ->CYCCNT;
		  count++;	
//发送里程计数据
//		if(count%4==0)
//		{
//			SendOdomposition(&Odom_Data_Z);
//		}
			
//哨兵决策		

		//根据不同方判断前哨战血量
		if(Referee_robotrx__all.robot_status.robot_id==7)
		{
			outpos_enmyhp=Referee_robotrx__all.game_robot_HP.blue_outpost_HP;
			outpos_selfhp=Referee_robotrx__all.game_robot_HP.red_outpost_HP;
		}
		else
		{
			outpos_enmyhp = Referee_robotrx__all.game_robot_HP.red_outpost_HP;
			outpos_selfhp = Referee_robotrx__all.game_robot_HP.blue_outpost_HP;
		}
		
		//记录每分钟允许补弹数
		if(Referee_robotrx__all.game_state.stage_remain_time%60==0)
			allowance_17mm_flag++;
		else if (Referee_robotrx__all.game_state.stage_remain_time<=0)
			allowance_17mm_flag=0;		
		
		
//判断比赛开始		
	  if((Referee_robotrx__all.game_state.game_progress ) == 4 )
		{
			referee_flag.game_start_flag=1;
			referee_flag.anti_engineer_flag=1;
//			referee_flag.tower_flag=1;		
		}
		else 
		{
			referee_flag.game_start_flag=0;
			referee_flag.anti_engineer_flag=0;
			referee_flag.outpost_smash_flag=0;
			
			referee_flag.tower_flag=0;
			referee_flag.rotate_flag=0;
			referee_flag.anti_outpost_flag=0;		

			
			step_defense=0;		
			step_outpost=0;
			step_dog=0;
			

			outpost_time =0;
			
		}
		
		
			if(outpos_selfhp<=0)
			{
				referee_flag.rotate_flag =1;				
			}
//补血		
    if(step_blood==0 && referee_flag.game_start_flag==1)
			{
				 if(Referee_robotrx__all.robot_status.current_HP <150 && Referee_robotrx__all.robot_status.current_HP >0)
				 {
					 step_blood =1;
					 referee_flag.blood_flag=1;
				 }
			}
			
		else if(step_blood==1)
			{
				 if(Referee_robotrx__all.robot_status.current_HP >=400)
				 {
					 referee_flag.blood_flag=0;
					 
					 step_blood =0;
				 }
			 }
			 
//补弹和解锁发射机构
		if(step_projectile==0 &&referee_flag.game_start_flag==1)
		{
			//哨兵死亡
			 if(Referee_robotrx__all.robot_status.power_management_shooter_output ==0&&Referee_robotrx__all.robot_status.power_management_gimbal_output==0&& Referee_robotrx__all.robot_status.power_management_chassis_output==0) 
				{
					referee_flag.projectile_flag=1;
					step_projectile=1;
				}					
				//允许发弹量小于某个值
				if(Referee_robotrx__all.projectile_allowance.projectile_allowance_17mm<=10 && allowance_17mm_flag>0)
					{
						step_projectile=2;
						referee_flag.projectile_flag=1;
					}
			}		
			 else if(step_projectile==1)
			 {
				 if(Referee_robotrx__all.robot_status.power_management_shooter_output ==1)
				 {
					 	referee_flag.projectile_flag=0;
						step_projectile=0;
				 }
				
			 }
			 else if(step_projectile==2)
			 {
				 if(Referee_robotrx__all.projectile_allowance.projectile_allowance_17mm>90)
				 {
						step_projectile=0;
					 	referee_flag.projectile_flag=0;
				 }				 
			 }
			 
			
			 
			 
	
//去环高点
//				if(step_outpost==0 &&referee_flag.game_start_flag==1)
//				{	
//						if(outpos_enmyhp<=0)
//						{
//							referee_flag.anti_engineer_flag=0;
//							referee_flag.anti_outpost_flag=1;
//							step_outpost=1;
//						}
//				}	

//剩余点				
			 	if(step_dog==0)
				{					
					if(outpos_enmyhp<=0)
					{
						step_dog=1;
					}
				}
			 	else if(step_dog==1)		
				{
					referee_flag.outpost_smash_flag=1;
					//未清空
				}					

//  前哨战摧毁 己方防御 				
				if( step_defense==0 && referee_flag.game_start_flag==1 )	
				{
					if(outpos_selfhp<=0)
					{
							step_defense=1;	
							//记录被击毁时间
							outpost_time =Referee_robotrx__all.game_state.stage_remain_time;
					}		
					
					if(Referee_robotrx__all.game_state.stage_remain_time<=200)
					{
						if(outpos_selfhp<600)
						{
							referee_flag.tower_flag=1;	
						}
					}
						
				}
				else if (outpost_time>=181 && step_defense ==1)
				{
					if(outpost_time-180>=90)
					{
						if(Referee_robotrx__all.game_state.stage_remain_time < outpost_time-40)
						{
							step_defense =1;
							referee_flag.tower_flag=1;
						}
					}
					else if(outpost_time-180<90 && outpost_time-180>=20)
					{
						if(Referee_robotrx__all.game_state.stage_remain_time<=200)
						{
							step_defense =1;
							referee_flag.tower_flag=1;
						}
					}
					else 
					{
							step_defense =1;
							referee_flag.tower_flag=1;						
					}						
				}			
				else if(outpost_time <=180 && step_defense ==1)
				{
						step_defense =1;
						referee_flag.tower_flag=1;							
				}
					
				
//接收云台手点位 注意测试场地地图和雷达地图的映射关系				
				map_x=Referee_robotrx__all.map_command.target_position_x;
				map_y=Referee_robotrx__all.map_command.target_position_y;		

				if(map_x !=map_x_last || map_y != map_y_last)
				{
					referee_flag.operate_cotrol_flag=1;
					referee_flag.map_X=(map_x-6.83)*100;
					referee_flag.map_Y=(map_y-5.67)*100;
				}

		    map_x_last=map_x;
		    map_y_last=map_y;				



//****************哨兵自主决策数据*****************//
//读秒复活
	   if(Referee_robotrx__all.robot_status.current_HP<=0)
		 {
			 reborn=1;
		 }
		 else 
		 {
			 reborn=0;
		 }
				
					        			
////自主补弹决策
//		if(step_Bullet==0)		
//		{
//			if(step_projectile==1 &&Referee_robotrx__all.projectile_allowance.remaining_gold_coin>600)
//			{
////				Bullet_amount+=100;		
//				step_Bullet=1;
//				bullet_times++;
//			}
//		
//		}
//  	else if(step_Bullet==1)
//			{
//				if(Referee_robotrx__all.sentry_info.success_exchange_bullet ==100*bullet_times)
//				{
//					step_Bullet=0;
//				}
//			}
		
	if(count%25==0)
	{
//			Get_routeArray(Referee_robotrx__all.robot_pos.x,5,delta_array_x);
//			Get_routeArray(Referee_robotrx__all.robot_pos.y,5,delta_array_y);	
//	SentrySendSelfroute(0x03,Referee_robotrx__all.robot_pos.x,Referee_robotrx__all.robot_pos.y,delta_array_x,delta_array_y);		
//		SentrySendSelfroute(0x03,1*10,1*10,delta_array_x,delta_array_y);		
		
		Sendsentry_auto_cmd(reborn,remote_reborn,Bullet_amount,remote_Bullet_times,remote_Blood_times);	
	} 
		Sendrefereeflag2pc(&referee_flag);
		//发送给云台裁判系统数据

	
		//发送遥控器数据
//		CANtxmessage(&hcan2,CAN_REMOTE_RC_M,rc_ctrl.mouse.press_l,rc_ctrl.mouse.press_r,rc_ctrl.mouse.x,rc_ctrl.mouse.y);
		if(count %8==0)
		{
	 	 CANtxmessage(&hcan2,CAN_REMOTE_RC_C,rc_ctrl.rc.ch[0],rc_ctrl.rc.ch[1],rc_ctrl.rc.ch[2],rc_ctrl.rc.ch[3]);
			DWT_Delayus(50); 
		}
		if(count %10==0)
		{
		 CANtxmessage(&hcan2,CAN_REMOTE_RC_S,rc_ctrl.rc.s[0],rc_ctrl.rc.s[1],rc_ctrl.key.v,0);
			DWT_Delayus(50);
		}
		
		 if(Referee_robotrx__all.robot_status.robot_id==7)
			{
				if(Referee_robotrx__all.game_robot_HP.blue_outpost_HP<=0)
				{
					enemy_outpostarrive_flag=1;
				}
				else 
					enemy_outpostarrive_flag=0;
			}
			else
			{
				if(Referee_robotrx__all.game_robot_HP.red_outpost_HP<=0)
				{
					enemy_outpostarrive_flag=1;
				}					
				else
					enemy_outpostarrive_flag=0;
			}
		    //发送枪口热量冷却数据
			DWT_Delayus(50);
			CANtxmessage(&hcan2,CAN_Refereedata2Gimbal,Referee_robotrx__all.power_heat_data.shooter_17mm_1_barrel_heat,Referee_robotrx__all.power_heat_data.shooter_17mm_2_barrel_heat, \
		               enemy_outpostarrive_flag,Referee_robotrx__all.robot_status.power_management_shooter_output);


//		dt_odom =DWT_GetDeltaT(&cnt_last);
		vTaskDelay (8);
	}
}


//获取坐标增量数组
void Get_routeArray(float start_point,float target_point,int8_t delta_array[49])
{
	float delta;
	delta=(target_point-start_point)*10/49;  //dm
	
	for(int i=0;i<49;i++)
	{	
		
//		if(i==0)
//	  delta_array[i]=delta;
//		else
//		delta_array[i] =delta_array[i-1]+delta;
	  delta_array[i]=delta;		
	}	
}





